#include "src\precompiled.h"
#include "src\maths.h"

#define MATH_PI       3.14159265358979323846


plane::plane() : distance(0) {}
plane::plane( const vector3& normal, float d ) : normal(normal), distance(d) {}
plane::plane( float x, float y, float z, float d ) : normal( x, y, z ), distance(d) {}
plane::~plane() {}

void plane::normalise()
{
	float len;
	float inv;

	len = normal.length();
	if( len > 1e-08f )
	{
		inv = 1.0f / len;
		normal *= inv;
		distance *= inv;
	}
}

float plane::get_distance( const vector3& point )
{
	return normal.dot( point ) + distance;
}

side plane::get_side( const vector3& point )
{
	float distance = get_distance( point );

	if( distance > 0 )
		return POSITIVE_SIDE;
	else if( distance < 0 )
		return NEGATIVE_SIDE;

	return NO_SIDE;
}

side plane::get_side( const vector3& centre, const vector3& half_size )
{
	float distance;
	float max_distance;

	distance = get_distance( centre );
	max_distance = normal.dotabs( half_size );

	if( distance < -max_distance )
		return NEGATIVE_SIDE;
	else if( distance > max_distance )
		return POSITIVE_SIDE;

	return BOTH_SIDE;
}

//-----------------------------------------------------------------------------

vector2::vector2() : x(0), y(0) {}

vector2::vector2( float _x, float _y ) : x(_x), y(_y) {}

vector2::~vector2() {}

vector2 vector2::operator + ( const vector2& vector ) const
{
	return vector2( x + vector.x,
					y + vector.y );
}

vector2 vector2::operator - ( const vector2& vector ) const
{
	return vector2( x - vector.x,
					y - vector.y );
}

vector2 vector2::operator * ( const vector2& vector ) const
{
	return vector2( x * vector.x,
					y * vector.y );
}

vector2 vector2::operator * ( const float val ) const
{
		return vector2( x * val,
						y * val );
}

vector2 vector2::operator / ( const vector2& vector ) const
{
	return vector2( x / vector.x,
					y / vector.y );
}

vector2 vector2::operator / ( const float val ) const
{
	return vector2( x / val,
					y / val );
}

vector2& vector2::operator += ( const vector2& vector )
{
	x += vector.x;
	y += vector.y;

	return *this;
}

vector2& vector2::operator -= ( const vector2& vector )
{
	x -= vector.x;
	y -= vector.y;

	return *this;
}

vector2& vector2::operator *= ( const vector2& vector )
{
	x *= vector.x;
	y *= vector.y;

	return *this;
}

vector2& vector2::operator *= ( const float val )
{
	x *= val;
	y *= val;

	return *this;
}

vector2& vector2::operator /= ( const vector2& vector )
{
	x /= vector.x;
	y /= vector.y;

	return *this;
}

vector2& vector2::operator /= ( const float val )
{
	x /= val;
	y /= val;

	return *this;
}

bool vector2::operator == ( const vector2& vector ) const
{
	return ( x == vector.x && y == vector.y );
}

bool vector2::operator != ( const vector2& vector ) const
{
	return ( x != vector.x || y != vector.y );
}

float vector2::dot( const vector2& vector )
{
	return x * vector.x + y * vector.y;
}

float vector2::dotabs( const vector2& vector )
{
	return abs(x * vector.x) + abs(y * vector.y);
}

vector2 vector2::mid_point( const vector2& vec ) const
{
	return vector2(
		( x + vec.x ) * 0.5f,
		( y + vec.y ) * 0.5f );
}

void vector2::make_ceil( const vector2& vector )
{
	if( vector.x > x ) x = vector.x;
	if( vector.y > y ) y = vector.y;
}

void vector2::make_floor( const vector2& vector )
{
	if( vector.x < x ) x = vector.x;
	if( vector.y < y ) y = vector.y;
}

vector2& vector2::operator = ( const vector2& vector )
{
	x = vector.x;
	y = vector.y;

	return *this;
}

float vector2::operator [] ( const unsigned id )
{
	assert( id < 3 );
	return *(&x + id);
}

bool vector2::operator > ( const vector2& vector )
{
	return ( ( x > vector.x ) && ( y > vector.y ) );
}

bool vector2::operator < ( const vector2& vector )
{
	return ( ( x < vector.x ) && ( y < vector.y ) );
}

float vector2::length()
{
	return sqrt( x * x + y * y );
}

//-----------------------------------------------------------------------------

vector3::vector3() : x(0), y(0), z(0) {}

vector3::vector3( float _x, float _y, float _z ) : x(_x), y(_y), z(_z) {}

vector3::~vector3() {}

vector3 vector3::operator + ( const vector3& vector ) const
{
	return vector3( x + vector.x,
					y + vector.y,
					z + vector.x );
}

vector3 vector3::operator - ( const vector3& vector ) const
{
	return vector3( x - vector.x,
					y - vector.y,
					z - vector.z );
}

vector3 vector3::operator * ( const vector3& vector ) const
{
	return vector3( x * vector.x,
					y * vector.y,
					z * vector.z );
}

vector3 vector3::operator * ( const float val ) const
{
		return vector3( x * val,
						y * val,
						z * val );
}

vector3 vector3::operator / ( const vector3& vector ) const
{
	return vector3( x / vector.x,
					y / vector.y,
					z / vector.z );
}

vector3 vector3::operator / ( const float val ) const
{
	return vector3( x / val,
					y / val,
					z / val );
}

vector3& vector3::operator += ( const vector3& vector )
{
	x += vector.x;
	y += vector.y;
	z += vector.z;

	return *this;
}

vector3& vector3::operator -= ( const vector3& vector )
{
	x -= vector.x;
	y -= vector.y;
	z -= vector.z;

	return *this;
}

vector3& vector3::operator *= ( const vector3& vector )
{
	x *= vector.x;
	y *= vector.y;
	z *= vector.z;

	return *this;
}

vector3& vector3::operator *= ( const float val )
{
	x *= val;
	y *= val;
	z *= val;

	return *this;
}

vector3& vector3::operator /= ( const vector3& vector )
{
	x /= vector.x;
	y /= vector.y;
	z /= vector.z;

	return *this;
}

vector3& vector3::operator /= ( const float val )
{
	x /= val;
	y /= val;
	z /= val;

	return *this;
}

bool vector3::operator == ( const vector3& vector ) const
{
	return ( x == vector.x && y == vector.y && z == vector.z );
}

bool vector3::operator != ( const vector3& vector ) const
{
	return ( x != vector.x || y != vector.y || z != vector.z );
}

float vector3::dot( const vector3& vector )
{
	return x * vector.x + y * vector.y + z * vector.z;
}

vector3 vector3::cross(const vector3& vec ) const
{
	return vector3(
		y * vec.z - z * vec.y,
		z * vec.x - x * vec.z,
		x * vec.y - y * vec.x);
}

float vector3::dotabs( const vector3& vector )
{
	return abs(x * vector.x) + abs(y * vector.y) + abs(z * vector.z);
}

vector3 vector3::mid_point( const vector3& vec ) const
{
	return vector3(
		( x + vec.x ) * 0.5f,
		( y + vec.y ) * 0.5f,
		( z + vec.z ) * 0.5f );
}

void vector3::make_ceil( const vector3& vector )
{
	if( vector.x > x ) x = vector.x;
	if( vector.y > y ) y = vector.y;
	if( vector.z > z ) z = vector.z;
}

void vector3::make_floor( const vector3& vector )
{
	if( vector.x < x ) x = vector.x;
	if( vector.y < y ) y = vector.y;
	if( vector.z < z ) z = vector.z;
}

vector3& vector3::operator = ( const vector3& vector )
{
	x = vector.x;
	y = vector.y;
	z = vector.z;

	return *this;
}

float &vector3::operator [] ( const unsigned id )
{
	assert( id < 3 );
	return *(&x + id);
}

bool vector3::operator > ( const vector3& vector )
{
	return ( ( x > vector.x ) && ( y > vector.y ) && ( z > vector.z ) );
}

bool vector3::operator < ( const vector3& vector )
{
	return ( ( x < vector.x ) && ( y < vector.y ) && ( z < vector.z ) );
}

float vector3::length()
{
	return sqrt( x * x + y * y + z * z );
}

float vector3::normalize()
{
	float length = sqrt( x * x + y * y + z * z );
	
	// Will also work for zero-sized vectors, but will change nothing
	if (length > 1e-08) {
	    float inv_length = 1.0f / length;
	    x *= inv_length;
	    y *= inv_length;
	    z *= inv_length;
	}
	return length;
}

//-----------------------------------------------------------------------------

vector4::vector4() : x(0), y(0), z(0), w(0) {}

vector4::vector4( float _x, float _y, float _z, float _w ) : x(_x), y(_y), z(_z), w(_w) {}

vector4::vector4(const vector3 &v)
{
	x = v.x;
	y = v.y;
	z = v.z;
	w = 0.0f;
}

vector4::vector4(const vector2 &v)
{
	x = v.x;
	y = v.y;
	z = 0.0f;
	w = 0.0f;
}

vector4::~vector4() {}

vector4 vector4::operator + ( const vector4& vector ) const
{
	return vector4( x + vector.x,
					y + vector.y,
					z + vector.z,
					w + vector.w );
}

vector4 vector4::operator - ( const vector4& vector ) const
{
	return vector4( x - vector.x,
					y - vector.y,
					z - vector.z,
					w - vector.w );
}

vector4 vector4::operator * ( const vector4& vector ) const
{
	return vector4( x * vector.x,
					y * vector.y,
					z * vector.z,
					w * vector.w );
}

vector4 vector4::operator * ( const float val ) const
{
		return vector4( x * val,
						y * val,
						z * val,
						w * val );
}

vector4 vector4::operator / ( const vector4& vector ) const
{
	return vector4( x / vector.x,
					y / vector.y,
					z / vector.z,
					w / vector.w );
}

vector4 vector4::operator / ( const float val ) const
{
	return vector4( x / val,
					y / val,
					z / val,
					w / val );
}

vector4& vector4::operator += ( const vector4& vector )
{
	x += vector.x;
	y += vector.y;
	z += vector.x;
	w += vector.w;

	return *this;
}

vector4& vector4::operator -= ( const vector4& vector )
{
	x -= vector.x;
	y -= vector.y;
	z -= vector.x;
	w -= vector.w;

	return *this;
}

vector4& vector4::operator *= ( const vector4& vector )
{
	x *= vector.x;
	y *= vector.y;
	z *= vector.z;
	w *= vector.w;

	return *this;
}

vector4& vector4::operator *= ( const float val )
{
	x *= val;
	y *= val;
	z *= val;
	w *= val;

	return *this;
}

vector4& vector4::operator /= ( const vector4& vector )
{
	x /= vector.x;
	y /= vector.y;
	z /= vector.z;
	w /= vector.w;

	return *this;
}

vector4& vector4::operator /= ( const float val )
{
	x /= val;
	y /= val;
	z /= val;
	w /= val;

	return *this;
}

bool vector4::operator == ( const vector4& vector ) const
{
	return ( x == vector.x && y == vector.y && z == vector.z && w == vector.w );
}

bool vector4::operator != ( const vector4& vector ) const
{
	return ( x != vector.x || y != vector.y || z != vector.z || w != vector.w );
}

float vector4::dot( const vector4& vector )
{
	return x * vector.x + y * vector.y + z * vector.z + w * vector.w;
}

float vector4::dotabs( const vector4& vector )
{
	return abs(x * vector.x) + abs(y * vector.y) + abs(z * vector.z) + abs(w * vector.w);
}

vector4& vector4::operator = ( const vector4& vector )
{
	x = vector.x;
	y = vector.y;
	z = vector.z;
	w = vector.w;

	return *this;
}

float vector4::operator [] ( const unsigned id )
{
	assert( id < 4 );
	return *(&x + id);
}

bool vector4::operator > ( const vector4& vector )
{
	return ( ( x > vector.x ) && ( y > vector.y ) && ( z > vector.z ) && ( w > vector.w ) );
}

bool vector4::operator < ( const vector4& vector )
{
	return ( ( x < vector.x ) && ( y < vector.y ) && ( z < vector.z ) && ( w < vector.w ) );
}

//-----------------------------------------------------------------------------

matrix4::matrix4() { identity(); }

matrix4::matrix4( const matrix4& m )
{
	operator=( m );
}

matrix4::matrix4( float m00, float m01, float m02, float m03,
		float m10, float m11, float m12, float m13,
		float m20, float m21, float m22, float m23,
		float m30, float m31, float m32, float m33 )
{
	m_m[0][0] = m00; m_m[0][1] = m01; m_m[0][2] = m02; m_m[0][3] = m03;
	m_m[1][0] = m10; m_m[1][1] = m11; m_m[1][2] = m12; m_m[1][3] = m13;
	m_m[2][0] = m20; m_m[2][1] = m21; m_m[2][2] = m22; m_m[2][3] = m23;
	m_m[3][0] = m30; m_m[3][1] = m31; m_m[3][2] = m32; m_m[3][3] = m33;
}

matrix4::matrix4( float *matrix ) { memcpy(m_m[0], matrix, sizeof(float) * 16); }

matrix4::~matrix4() {}

void matrix4::identity()
{
	m_m[0][0] = 1; m_m[0][1] = 0; m_m[0][2] = 0; m_m[0][3] = 0;
	m_m[1][0] = 0; m_m[1][1] = 1; m_m[1][2] = 0; m_m[1][3] = 0;
	m_m[2][0] = 0; m_m[2][1] = 0; m_m[2][2] = 1; m_m[2][3] = 0;
	m_m[3][0] = 0; m_m[3][1] = 0; m_m[3][2] = 0; m_m[3][3] = 1;
}

matrix4 matrix4::concatenate( const matrix4 &m2 ) const
{
	matrix4 r;
	r.m_m[0][0] = m_m[0][0] * m2.m_m[0][0] + m_m[0][1] * m2.m_m[1][0] + m_m[0][2] * m2.m_m[2][0] + m_m[0][3] * m2.m_m[3][0];
	r.m_m[0][1] = m_m[0][0] * m2.m_m[0][1] + m_m[0][1] * m2.m_m[1][1] + m_m[0][2] * m2.m_m[2][1] + m_m[0][3] * m2.m_m[3][1];
	r.m_m[0][2] = m_m[0][0] * m2.m_m[0][2] + m_m[0][1] * m2.m_m[1][2] + m_m[0][2] * m2.m_m[2][2] + m_m[0][3] * m2.m_m[3][2];
	r.m_m[0][3] = m_m[0][0] * m2.m_m[0][3] + m_m[0][1] * m2.m_m[1][3] + m_m[0][2] * m2.m_m[2][3] + m_m[0][3] * m2.m_m[3][3];

	r.m_m[1][0] = m_m[1][0] * m2.m_m[0][0] + m_m[1][1] * m2.m_m[1][0] + m_m[1][2] * m2.m_m[2][0] + m_m[1][3] * m2.m_m[3][0];
	r.m_m[1][1] = m_m[1][0] * m2.m_m[0][1] + m_m[1][1] * m2.m_m[1][1] + m_m[1][2] * m2.m_m[2][1] + m_m[1][3] * m2.m_m[3][1];
	r.m_m[1][2] = m_m[1][0] * m2.m_m[0][2] + m_m[1][1] * m2.m_m[1][2] + m_m[1][2] * m2.m_m[2][2] + m_m[1][3] * m2.m_m[3][2];
	r.m_m[1][3] = m_m[1][0] * m2.m_m[0][3] + m_m[1][1] * m2.m_m[1][3] + m_m[1][2] * m2.m_m[2][3] + m_m[1][3] * m2.m_m[3][3];

	r.m_m[2][0] = m_m[2][0] * m2.m_m[0][0] + m_m[2][1] * m2.m_m[1][0] + m_m[2][2] * m2.m_m[2][0] + m_m[2][3] * m2.m_m[3][0];
	r.m_m[2][1] = m_m[2][0] * m2.m_m[0][1] + m_m[2][1] * m2.m_m[1][1] + m_m[2][2] * m2.m_m[2][1] + m_m[2][3] * m2.m_m[3][1];
	r.m_m[2][2] = m_m[2][0] * m2.m_m[0][2] + m_m[2][1] * m2.m_m[1][2] + m_m[2][2] * m2.m_m[2][2] + m_m[2][3] * m2.m_m[3][2];
	r.m_m[2][3] = m_m[2][0] * m2.m_m[0][3] + m_m[2][1] * m2.m_m[1][3] + m_m[2][2] * m2.m_m[2][3] + m_m[2][3] * m2.m_m[3][3];

	r.m_m[3][0] = m_m[3][0] * m2.m_m[0][0] + m_m[3][1] * m2.m_m[1][0] + m_m[3][2] * m2.m_m[2][0] + m_m[3][3] * m2.m_m[3][0];
	r.m_m[3][1] = m_m[3][0] * m2.m_m[0][1] + m_m[3][1] * m2.m_m[1][1] + m_m[3][2] * m2.m_m[2][1] + m_m[3][3] * m2.m_m[3][1];
	r.m_m[3][2] = m_m[3][0] * m2.m_m[0][2] + m_m[3][1] * m2.m_m[1][2] + m_m[3][2] * m2.m_m[2][2] + m_m[3][3] * m2.m_m[3][2];
	r.m_m[3][3] = m_m[3][0] * m2.m_m[0][3] + m_m[3][1] * m2.m_m[1][3] + m_m[3][2] * m2.m_m[2][3] + m_m[3][3] * m2.m_m[3][3];

	return r;
}

vector3 matrix4::transform_affine(  const vector3& v) const
{
	assert( is_affine() );

	return vector3(
		m_m[0][0] * v.x + m_m[0][1] * v.y + m_m[0][2] * v.z + m_m[0][3], 
		m_m[1][0] * v.x + m_m[1][1] * v.y + m_m[1][2] * v.z + m_m[1][3],
		m_m[2][0] * v.x + m_m[2][1] * v.y + m_m[2][2] * v.z + m_m[2][3]);
}

bool matrix4::is_affine(void) const
{
	return ( ( m_m[3][0] == 0 ) && ( m_m[3][1] == 0 ) && ( m_m[3][2] == 0 ) && ( m_m[3][3] == 1 ) );
}

matrix4 matrix4::operator * ( float mul ) const
{
	return matrix4( m_m[0][0] * mul, m_m[1][0] * mul, m_m[2][0] * mul, m_m[3][0] * mul,
					m_m[0][1] * mul, m_m[1][1] * mul, m_m[2][1] * mul, m_m[3][1] * mul,
					m_m[0][2] * mul, m_m[1][2] * mul, m_m[2][2] * mul, m_m[3][2] * mul,
					m_m[0][3] * mul, m_m[1][3] * mul, m_m[2][3] * mul, m_m[3][3] * mul );
}

matrix4 matrix4::operator * ( const matrix4& m ) const
{
	return concatenate( m );
}

vector3 matrix4::operator * ( const vector3& v ) const
{
	vector3 r;
	float inv;

	inv = 1.0f / ( m_m[3][0] * v.x + m_m[3][1] * v.y + m_m[3][2] * v.z + m_m[3][3] );

	r.x = ( m_m[0][0] * v.x + m_m[0][1] * v.y + m_m[0][2] * v.z + m_m[0][3] ) * inv;
	r.y = ( m_m[1][0] * v.x + m_m[1][1] * v.y + m_m[1][2] * v.z + m_m[1][3] ) * inv;
	r.z = ( m_m[2][0] * v.x + m_m[2][1] * v.y + m_m[2][2] * v.z + m_m[2][3] ) * inv;

	return r;
}

matrix4 matrix4::operator + ( const matrix4 &m ) const
{
	matrix4 r;

	r.m_m[0][0] = m_m[0][0] + m.m_m[0][0];
	r.m_m[0][1] = m_m[0][1] + m.m_m[0][1];
	r.m_m[0][2] = m_m[0][2] + m.m_m[0][2];
	r.m_m[0][3] = m_m[0][3] + m.m_m[0][3];

	r.m_m[1][0] = m_m[1][0] + m.m_m[1][0];
	r.m_m[1][1] = m_m[1][1] + m.m_m[1][1];
	r.m_m[1][2] = m_m[1][2] + m.m_m[1][2];
	r.m_m[1][3] = m_m[1][3] + m.m_m[1][3];

	r.m_m[2][0] = m_m[2][0] + m.m_m[2][0];
	r.m_m[2][1] = m_m[2][1] + m.m_m[2][1];
	r.m_m[2][2] = m_m[2][2] + m.m_m[2][2];
	r.m_m[2][3] = m_m[2][3] + m.m_m[2][3];

	r.m_m[3][0] = m_m[3][0] + m.m_m[3][0];
	r.m_m[3][1] = m_m[3][1] + m.m_m[3][1];
	r.m_m[3][2] = m_m[3][2] + m.m_m[3][2];
	r.m_m[3][3] = m_m[3][3] + m.m_m[3][3];

	return r;
}

matrix4 matrix4::operator - ( const matrix4 &m ) const
{
	matrix4 r;

	r.m_m[0][0] = m_m[0][0] - m.m_m[0][0];
	r.m_m[0][1] = m_m[0][1] - m.m_m[0][1];
	r.m_m[0][2] = m_m[0][2] - m.m_m[0][2];
	r.m_m[0][3] = m_m[0][3] - m.m_m[0][3];

	r.m_m[1][0] = m_m[1][0] - m.m_m[1][0];
	r.m_m[1][1] = m_m[1][1] - m.m_m[1][1];
	r.m_m[1][2] = m_m[1][2] - m.m_m[1][2];
	r.m_m[1][3] = m_m[1][3] - m.m_m[1][3];

	r.m_m[2][0] = m_m[2][0] - m.m_m[2][0];
	r.m_m[2][1] = m_m[2][1] - m.m_m[2][1];
	r.m_m[2][2] = m_m[2][2] - m.m_m[2][2];
	r.m_m[2][3] = m_m[2][3] - m.m_m[2][3];

	r.m_m[3][0] = m_m[3][0] - m.m_m[3][0];
	r.m_m[3][1] = m_m[3][1] - m.m_m[3][1];
	r.m_m[3][2] = m_m[3][2] - m.m_m[3][2];
	r.m_m[3][3] = m_m[3][3] - m.m_m[3][3];

	return r;
}

bool matrix4::operator == ( const matrix4& m ) const
{
	if( 
		m_m[0][0] != m.m_m[0][0] || m_m[0][1] != m.m_m[0][1] || m_m[0][2] != m.m_m[0][2] || m_m[0][3] != m.m_m[0][3] ||
		m_m[1][0] != m.m_m[1][0] || m_m[1][1] != m.m_m[1][1] || m_m[1][2] != m.m_m[1][2] || m_m[1][3] != m.m_m[1][3] ||
		m_m[2][0] != m.m_m[2][0] || m_m[2][1] != m.m_m[2][1] || m_m[2][2] != m.m_m[2][2] || m_m[2][3] != m.m_m[2][3] ||
		m_m[3][0] != m.m_m[3][0] || m_m[3][1] != m.m_m[3][1] || m_m[3][2] != m.m_m[3][2] || m_m[3][3] != m.m_m[3][3] )
		return false;
	return true;
}

bool matrix4::operator != ( const matrix4& m ) const
{
	if( 
		m_m[0][0] != m.m_m[0][0] || m_m[0][1] != m.m_m[0][1] || m_m[0][2] != m.m_m[0][2] || m_m[0][3] != m.m_m[0][3] ||
		m_m[1][0] != m.m_m[1][0] || m_m[1][1] != m.m_m[1][1] || m_m[1][2] != m.m_m[1][2] || m_m[1][3] != m.m_m[1][3] ||
		m_m[2][0] != m.m_m[2][0] || m_m[2][1] != m.m_m[2][1] || m_m[2][2] != m.m_m[2][2] || m_m[2][3] != m.m_m[2][3] ||
		m_m[3][0] != m.m_m[3][0] || m_m[3][1] != m.m_m[3][1] || m_m[3][2] != m.m_m[3][2] || m_m[3][3] != m.m_m[3][3] )
		return true;
	return false;
}

void matrix4::operator = ( const matrix4& m )
{
	m_m[0][0] = m.m_m[0][0]; m_m[0][1] = m.m_m[0][1]; m_m[0][2] = m.m_m[0][2]; m_m[0][3] = m.m_m[0][3];
	m_m[1][0] = m.m_m[1][0]; m_m[1][1] = m.m_m[1][1]; m_m[1][2] = m.m_m[1][2]; m_m[1][3] = m.m_m[1][3];
	m_m[2][0] = m.m_m[2][0]; m_m[2][1] = m.m_m[2][1]; m_m[2][2] = m.m_m[2][2]; m_m[2][3] = m.m_m[2][3];
	m_m[3][0] = m.m_m[3][0]; m_m[3][1] = m.m_m[3][1]; m_m[3][2] = m.m_m[3][2]; m_m[3][3] = m.m_m[3][3];
}

float* matrix4::operator [] ( size_t nrow )
{
	assert( nrow < 4 );
	return m_m[nrow];
}

const float* matrix4::operator [] ( size_t nrow ) const
{
	assert( nrow < 4 );
	return m_m[nrow];
}

matrix4 matrix4::transpose() const
{
	return matrix4(	m_m[0][0], m_m[1][0], m_m[2][0], m_m[3][0],
					m_m[0][1], m_m[1][1], m_m[2][1], m_m[3][1],
					m_m[0][2], m_m[1][2], m_m[2][2], m_m[3][2],
					m_m[0][3], m_m[1][3], m_m[2][3], m_m[3][3] );
}

void matrix4::decomposition(vector3& position, vector3& scale, quaternion& orientation) const
{
	assert(is_affine());

	matrix3 m3x3;

	m3x3.m_m[0][0] = m_m[0][0];
	m3x3.m_m[0][1] = m_m[0][1];
	m3x3.m_m[0][2] = m_m[0][2];
	m3x3.m_m[1][0] = m_m[1][0];
	m3x3.m_m[1][1] = m_m[1][1];
	m3x3.m_m[1][2] = m_m[1][2];
	m3x3.m_m[2][0] = m_m[2][0];
	m3x3.m_m[2][1] = m_m[2][1];
	m3x3.m_m[2][2] = m_m[2][2];

	matrix3 matQ;
	vector3 vecU;
	m3x3.QDUDecomposition( matQ, scale, vecU ); 

	orientation = quaternion( matQ );
	position = vector3( m_m[0][3], m_m[1][3], m_m[2][3] );
}

matrix3 matrix4::extract_matrix3() const
{
	matrix3 m3x3;
	m3x3[0][0] = m_m[0][0];
	m3x3[0][1] = m_m[0][1];
	m3x3[0][2] = m_m[0][2];
	m3x3[1][0] = m_m[1][0];
	m3x3[1][1] = m_m[1][1];
	m3x3[1][2] = m_m[1][2];
	m3x3[2][0] = m_m[2][0];
	m3x3[2][1] = m_m[2][1];
	m3x3[2][2] = m_m[2][2];
	return m3x3;
}

vector3 matrix4::extract_translation() const { return vector3(m_m[0][3], m_m[1][3], m_m[2][3]); }

matrix4 matrix4::inverse() const
{
	float m00 = m_m[0][0], m01 = m_m[0][1], m02 = m_m[0][2], m03 = m_m[0][3];
	float m10 = m_m[1][0], m11 = m_m[1][1], m12 = m_m[1][2], m13 = m_m[1][3];
	float m20 = m_m[2][0], m21 = m_m[2][1], m22 = m_m[2][2], m23 = m_m[2][3];
	float m30 = m_m[3][0], m31 = m_m[3][1], m32 = m_m[3][2], m33 = m_m[3][3];
	
	float v0 = m20 * m31 - m21 * m30;
	float v1 = m20 * m32 - m22 * m30;
	float v2 = m20 * m33 - m23 * m30;
	float v3 = m21 * m32 - m22 * m31;
	float v4 = m21 * m33 - m23 * m31;
	float v5 = m22 * m33 - m23 * m32;
	
	float t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
	float t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
	float t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
	float t30 = - (v3 * m10 - v1 * m11 + v0 * m12);
	
	float inv_det = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);
	
	float d00 = t00 * inv_det;
	float d10 = t10 * inv_det;
	float d20 = t20 * inv_det;
	float d30 = t30 * inv_det;
	
	float d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * inv_det;
	float d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * inv_det;
	float d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * inv_det;
	float d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * inv_det;
	
	v0 = m10 * m31 - m11 * m30;
	v1 = m10 * m32 - m12 * m30;
	v2 = m10 * m33 - m13 * m30;
	v3 = m11 * m32 - m12 * m31;
	v4 = m11 * m33 - m13 * m31;
	v5 = m12 * m33 - m13 * m32;
	
	float d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * inv_det;
	float d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * inv_det;
	float d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * inv_det;
	float d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * inv_det;
	
	v0 = m21 * m10 - m20 * m11;
	v1 = m22 * m10 - m20 * m12;
	v2 = m23 * m10 - m20 * m13;
	v3 = m22 * m11 - m21 * m12;
	v4 = m23 * m11 - m21 * m13;
	v5 = m23 * m12 - m22 * m13;
	
	float d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * inv_det;
	float d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * inv_det;
	float d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * inv_det;
	float d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * inv_det;
	
	return matrix4(
	    d00, d01, d02, d03,
	    d10, d11, d12, d13,
	    d20, d21, d22, d23,
	    d30, d31, d32, d33);
}

void matrix4::make_transform(const vector3& position, const vector3& scale, const quaternion& orientation)
{	
	matrix3 rot3x3;
	orientation.to_rotation_matrix(rot3x3);
	
	// Set up final matrix with scale, rotation and translation
	m_m[0][0] = scale.x * rot3x3[0][0]; m_m[0][1] = scale.y * rot3x3[0][1]; m_m[0][2] = scale.z * rot3x3[0][2]; m_m[0][3] = position.x;
	m_m[1][0] = scale.x * rot3x3[1][0]; m_m[1][1] = scale.y * rot3x3[1][1]; m_m[1][2] = scale.z * rot3x3[1][2]; m_m[1][3] = position.y;
	m_m[2][0] = scale.x * rot3x3[2][0]; m_m[2][1] = scale.y * rot3x3[2][1]; m_m[2][2] = scale.z * rot3x3[2][2]; m_m[2][3] = position.z;
	
	// No projection term_m
	m_m[3][0] = 0; m_m[3][1] = 0; m_m[3][2] = 0; m_m[3][3] = 1;
}

//-----------------------------------------------------------------------------

bounding_box::bounding_box()
{
	m_min = vector3( -0.5, -0.5, -0.5 );
	m_max = vector3( 0.5, 0.5, 0.5 );
	m_extents = EXTENT_NULL;
}

bounding_box::bounding_box( const vector3& min, const vector3& max )
{
	set( min, max );
}

bounding_box::bounding_box( float mx, float my, float mz, float Mx, float My, float Mz )
{
	set( mx, my, mz, Mx, My, Mz );
}

bounding_box::~bounding_box() {}

void bounding_box::set( const vector3& min, const vector3& max )
{
	set( min.x, min.y, min.z, max.x, max.y, max.z );
}

void bounding_box::set( float mx, float my, float mz, float Mx, float My, float Mz )
{
//	assert( (mx < Mx) && ( my < My ) && (mz < Mz ) ); // kinda hack!

	m_extents = EXTENT_FINITE;

	m_min.x = mx;
	m_min.y = my;
	m_min.z = mz;

	m_max.x = Mx;
	m_max.y = My;
	m_max.z = Mz;
}

void bounding_box::merge( const bounding_box& bbox )
{
	vector3 min, max;

	if( ( bbox.is_null() ) || ( is_infinite() ) )
	{
		return;
	}
	else if( bbox.is_infinite() )
	{
		m_extents = EXTENT_INFINITE;
	}
	else if( is_null() )
	{
		set( bbox.m_min, bbox.m_max );
	}
	else
	{
		min = m_min;
		max = m_max;

		max.make_ceil( bbox.m_max );
		min.make_floor( bbox.m_min );

		set( min, max );
	}
}

vector3 bounding_box::get_minimum() const
{
	return m_min;
}

vector3 bounding_box::get_maximum() const
{
	return m_max;
}

vector3 bounding_box::get_centre() const
{
	return vector3(
		( abs( m_max.x ) - abs( m_min.x ) ) * 0.5f,
		( abs( m_max.y ) - abs( m_min.y ) ) * 0.5f,
		( abs( m_max.z ) - abs( m_min.z ) ) * 0.5f );
}

vector3 bounding_box::get_half_size() const
{
	return vector3( m_max - m_min ) * 0.5f;
}

vector3 bounding_box::get_size() const
{
	return vector3( m_max - m_min );
}

void bounding_box::set_null()
{
	m_extents = EXTENT_NULL;
}

bool bounding_box::is_null() const
{
	return (m_extents == EXTENT_NULL) ? true : false;
}

bool bounding_box::is_finite() const
{
	return (m_extents == EXTENT_FINITE) ? true : false;
}

bool bounding_box::is_infinite() const
{
	return (m_extents == EXTENT_INFINITE) ? true : false;
}

bounding_box& bounding_box::operator = ( const bounding_box& bbox )
{
	vector3 min, max;

	min = bbox.get_minimum();
	max = bbox.get_maximum();

	set( min, max );

	return *this;
}

void bounding_box::transform_affine( const matrix4& m )
{
	assert( m.is_affine() );

	vector3 centre;
	vector3 half_size;
	vector3 new_centre;

	if( m_extents != EXTENT_FINITE )
	{
		return;
	}

	centre = get_centre();
	half_size = get_half_size();

	new_centre = m.transform_affine( centre );

	vector3 new_half_size(
		abs(m[0][0]) * half_size.x + abs(m[0][1]) * half_size.y + abs(m[0][2]) * half_size.z, 
		abs(m[1][0]) * half_size.x + abs(m[1][1]) * half_size.y + abs(m[1][2]) * half_size.z,
		abs(m[2][0]) * half_size.x + abs(m[2][1]) * half_size.y + abs(m[2][2]) * half_size.z);

	set( new_centre - new_half_size, new_centre + new_half_size );
}

//-----------------------------------------------------------------------------

matrix3::matrix3() { identity(); }

matrix3::matrix3( float m00, float m01, float m02,
		float m10, float m11, float m12,
		 float m20, float m21, float m22 )
{
	m_m[0][0] = m00; m_m[0][1] = m01; m_m[0][2] = m02;
	m_m[1][0] = m10; m_m[1][1] = m11; m_m[1][2] = m12;
	m_m[2][0] = m20; m_m[2][1] = m21; m_m[2][2] = m22;
}

matrix3::matrix3( const matrix3& m )
{
	operator=( m );
}

matrix3::~matrix3() {}

void matrix3::identity()
{
	m_m[0][0] = 1; m_m[0][1] = 0; m_m[0][2] = 0;
	m_m[1][0] = 0; m_m[1][1] = 1; m_m[1][2] = 0;
	m_m[2][0] = 0; m_m[2][1] = 0; m_m[2][2] = 1;
}

float* matrix3::operator [] ( size_t n )
{
	assert( n < 3 );
	return m_m[n];
}

const float* matrix3::operator [] ( size_t n ) const
{
	assert( n < 3 );
	return m_m[n];
}

void matrix3::operator = ( const matrix3& m )
{
	m_m[0][0] = m.m_m[0][0]; m_m[0][1] = m.m_m[0][1]; m_m[0][2] = m.m_m[0][2];
	m_m[1][0] = m.m_m[1][0]; m_m[1][1] = m.m_m[1][1]; m_m[1][2] = m.m_m[1][2];
	m_m[2][0] = m.m_m[2][0]; m_m[2][1] = m.m_m[2][1]; m_m[2][2] = m.m_m[2][2];
}

bool matrix3::operator != ( const matrix3& m ) const
{
	if( 
	m_m[0][0] != m.m_m[0][0] || m_m[0][1] != m.m_m[0][1] || m_m[0][2] != m.m_m[0][2] ||
	m_m[1][0] != m.m_m[1][0] || m_m[1][1] != m.m_m[1][1] || m_m[1][2] != m.m_m[1][2] ||
	m_m[2][0] != m.m_m[2][0] || m_m[2][1] != m.m_m[2][1] || m_m[2][2] != m.m_m[2][2] )
	return true;
	return false;
}

bool matrix3::operator == ( const matrix3& m ) const
{
	if( 
		m_m[0][0] != m.m_m[0][0] || m_m[0][1] != m.m_m[0][1] || m_m[0][2] != m.m_m[0][2] ||
		m_m[1][0] != m.m_m[1][0] || m_m[1][1] != m.m_m[1][1] || m_m[1][2] != m.m_m[1][2] ||
		m_m[2][0] != m.m_m[2][0] || m_m[2][1] != m.m_m[2][1] || m_m[2][2] != m.m_m[2][2] )
		return false;
	return true;
}

matrix3 matrix3::concatenate( const matrix3& m ) const
{
	matrix3 r;

	r.m_m[0][0] = m_m[0][0] * m.m_m[0][0] + m_m[0][1] * m.m_m[1][0] + m_m[0][2] * m.m_m[2][0];
	r.m_m[0][1] = m_m[0][0] * m.m_m[0][1] + m_m[0][1] * m.m_m[1][1] + m_m[0][2] * m.m_m[2][1];
	r.m_m[0][2] = m_m[0][0] * m.m_m[0][2] + m_m[0][1] * m.m_m[1][2] + m_m[0][2] * m.m_m[2][2];

	r.m_m[1][0] = m_m[1][0] * m.m_m[0][0] + m_m[1][1] * m.m_m[1][0] + m_m[1][2] * m.m_m[2][0];
	r.m_m[1][1] = m_m[1][0] * m.m_m[0][1] + m_m[1][1] * m.m_m[1][1] + m_m[1][2] * m.m_m[2][1];
	r.m_m[1][2] = m_m[1][0] * m.m_m[0][2] + m_m[1][1] * m.m_m[1][2] + m_m[1][2] * m.m_m[2][2];

	r.m_m[2][0] = m_m[2][0] * m.m_m[0][0] + m_m[2][1] * m.m_m[1][0] + m_m[2][2] * m.m_m[2][0];
	r.m_m[2][1] = m_m[2][0] * m.m_m[0][1] + m_m[2][1] * m.m_m[1][1] + m_m[2][2] * m.m_m[2][1];
	r.m_m[2][2] = m_m[2][0] * m.m_m[0][2] + m_m[2][1] * m.m_m[1][2] + m_m[2][2] * m.m_m[2][2];

	return r;
}

matrix3 matrix3::operator * ( float mul ) const
{
	return matrix3( m_m[0][0] * mul, m_m[0][1] * mul, m_m[0][2] * mul,
					m_m[1][0] * mul, m_m[1][1] * mul, m_m[1][2] * mul,
					m_m[2][0] * mul, m_m[2][1] * mul, m_m[2][2] * mul );
}

matrix3 matrix3::operator * ( const matrix3& m ) const
{
	return concatenate( m );
}

vector3 matrix3::operator * ( const vector3& v )
{
	return vector3( m_m[0][0] * v.x + m_m[0][1] * v.y + m_m[0][2] * v.z,
					m_m[1][0] * v.x + m_m[1][1] * v.y + m_m[1][2] * v.z,
					m_m[2][0] * v.x + m_m[2][1] * v.y + m_m[2][2] * v.z );
}

matrix3 matrix3::operator + ( const matrix3& m )
{
	matrix3 r;

	r.m_m[0][0] = m_m[0][0] + m.m_m[0][0];
	r.m_m[0][1] = m_m[0][1] + m.m_m[0][1];
	r.m_m[0][2] = m_m[0][2] + m.m_m[0][2];

	r.m_m[1][0] = m_m[1][0] + m.m_m[1][0];
	r.m_m[1][1] = m_m[1][1] + m.m_m[1][1];
	r.m_m[1][2] = m_m[1][2] + m.m_m[1][2];

	r.m_m[2][0] = m_m[2][0] + m.m_m[2][0];
	r.m_m[2][1] = m_m[2][1] + m.m_m[2][1];
	r.m_m[2][2] = m_m[2][2] + m.m_m[2][2];

	return r;
}

matrix3 matrix3::operator - ( const matrix3& m )
{
	matrix3 r;

	r.m_m[0][0] = m_m[0][0] - m.m_m[0][0];
	r.m_m[0][1] = m_m[0][1] - m.m_m[0][1];
	r.m_m[0][2] = m_m[0][2] - m.m_m[0][2];

	r.m_m[1][0] = m_m[1][0] - m.m_m[1][0];
	r.m_m[1][1] = m_m[1][1] - m.m_m[1][1];
	r.m_m[1][2] = m_m[1][2] - m.m_m[1][2];

	r.m_m[2][0] = m_m[2][0] - m.m_m[2][0];
	r.m_m[2][1] = m_m[2][1] - m.m_m[2][1];
	r.m_m[2][2] = m_m[2][2] - m.m_m[2][2];

	return r;
}

bool matrix3::to_euler_angles(float &yaw, float &pitch, float &roll) const
{	
	pitch = asin(m_m[0][2]);
	if (pitch < (MATH_PI / 2))
	{
	    if (pitch > -(MATH_PI / 2))
	    {
	        yaw = atan2(-m_m[1][2],m_m[2][2]);
	        roll = atan2(-m_m[0][1],m_m[0][0]);
	        return true;
	    }
	    else
	    {
	        // WARNING.  Not a unique solution.
	        float y = atan2(m_m[1][0],m_m[1][1]);
	        roll = 0.0;  // any angle works
	        yaw = roll - y;
	        return false;
	    }
	}
	else
	{
	    // WARNING.  Not a unique solution.
	    float y = atan2(m_m[1][0],m_m[1][1]);
	    roll = 0.0;  // any angle works
	    yaw = y - roll;
	    return false;
	}
}

void matrix3::QDUDecomposition(matrix3& kQ, vector3& kD, vector3& kU) const
{
	float fInvLength = 1.0f / sqrtf(m_m[0][0]*m_m[0][0]
		+ m_m[1][0]*m_m[1][0] +
		m_m[2][0]*m_m[2][0]);
	kQ[0][0] = m_m[0][0]*fInvLength;
	kQ[1][0] = m_m[1][0]*fInvLength;
	kQ[2][0] = m_m[2][0]*fInvLength;

	float fDot = kQ[0][0]*m_m[0][1] + kQ[1][0]*m_m[1][1] +
		kQ[2][0]*m_m[2][1];
	kQ[0][1] = m_m[0][1]-fDot*kQ[0][0];
	kQ[1][1] = m_m[1][1]-fDot*kQ[1][0];
	kQ[2][1] = m_m[2][1]-fDot*kQ[2][0];
	fInvLength = 1.0f / sqrtf(kQ[0][1]*kQ[0][1] + kQ[1][1]*kQ[1][1] +
		kQ[2][1]*kQ[2][1]);
	kQ[0][1] *= fInvLength;
	kQ[1][1] *= fInvLength;
	kQ[2][1] *= fInvLength;

	fDot = kQ[0][0]*m_m[0][2] + kQ[1][0]*m_m[1][2] +
		kQ[2][0]*m_m[2][2];
	kQ[0][2] = m_m[0][2]-fDot*kQ[0][0];
	kQ[1][2] = m_m[1][2]-fDot*kQ[1][0];
	kQ[2][2] = m_m[2][2]-fDot*kQ[2][0];
	fDot = kQ[0][1]*m_m[0][2] + kQ[1][1]*m_m[1][2] +
	    kQ[2][1]*m_m[2][2];
	kQ[0][2] -= fDot*kQ[0][1];
	kQ[1][2] -= fDot*kQ[1][1];
	kQ[2][2] -= fDot*kQ[2][1];
	fInvLength = 1.0f / sqrtf(kQ[0][2]*kQ[0][2] + kQ[1][2]*kQ[1][2] +
		kQ[2][2]*kQ[2][2]);
	kQ[0][2] *= fInvLength;
	kQ[1][2] *= fInvLength;
	kQ[2][2] *= fInvLength;

	// guarantee that orthogonal matrix has determinant 1 (no reflections)
	float fDet = kQ[0][0]*kQ[1][1]*kQ[2][2] + kQ[0][1]*kQ[1][2]*kQ[2][0] +
		kQ[0][2]*kQ[1][0]*kQ[2][1] - kQ[0][2]*kQ[1][1]*kQ[2][0] -
		kQ[0][1]*kQ[1][0]*kQ[2][2] - kQ[0][0]*kQ[1][2]*kQ[2][1];

	if ( fDet < 0.0 )
	{
		for (size_t iRow = 0; iRow < 3; iRow++)
			for (size_t iCol = 0; iCol < 3; iCol++)
				kQ[iRow][iCol] = -kQ[iRow][iCol];
	}

	// build "right" matrix R
	matrix3 kR;
	kR[0][0] = kQ[0][0]*m_m[0][0] + kQ[1][0]*m_m[1][0] +
		kQ[2][0]*m_m[2][0];
	kR[0][1] = kQ[0][0]*m_m[0][1] + kQ[1][0]*m_m[1][1] +
		kQ[2][0]*m_m[2][1];
	kR[1][1] = kQ[0][1]*m_m[0][1] + kQ[1][1]*m_m[1][1] +
		kQ[2][1]*m_m[2][1];
	kR[0][2] = kQ[0][0]*m_m[0][2] + kQ[1][0]*m_m[1][2] +
		kQ[2][0]*m_m[2][2];
	kR[1][2] = kQ[0][1]*m_m[0][2] + kQ[1][1]*m_m[1][2] +
		kQ[2][1]*m_m[2][2];
	kR[2][2] = kQ[0][2]*m_m[0][2] + kQ[1][2]*m_m[1][2] +
		kQ[2][2]*m_m[2][2];

	// the scaling component
	kD[0] = kR[0][0];
	kD[1] = kR[1][1];
	kD[2] = kR[2][2];

	// the shear component
	float fInvD0 = 1.0f/kD[0];
	kU[0] = kR[0][1]*fInvD0;
	kU[1] = kR[0][2]*fInvD0;
	kU[2] = kR[1][2]/kD[1];
}

//-----------------------------------------------------------------------------

quaternion::quaternion() : w(1), x(0), y(0), z(0) {}

quaternion::quaternion( const matrix3& m )
{
	from_rotation_matrix( m );
}

quaternion::quaternion( float angle, const vector3& axis )
{
	from_angle_axis( angle, axis );
}

quaternion::quaternion( float _w, float _x, float _y, float _z ) : w(_w), x(_x), y(_y), z(_z) {}
quaternion::~quaternion() {}

void quaternion::from_rotation_matrix( const matrix3& m )
{
	// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
	// article "Quaternion Calculus and Fast Animation".

	float fTrace = m[0][0]+m[1][1]+m[2][2];
	float fRoot;

	if ( fTrace > 0.0 )
	{
		// |w| > 1/2, may as well choose w > 1/2
		fRoot = sqrt(fTrace + 1.0f);  // 2w
		w = 0.5f*fRoot;
		fRoot = 0.5f/fRoot;  // 1/(4w)
		x = (m[2][1]-m[1][2])*fRoot;
		y = (m[0][2]-m[2][0])*fRoot;
		z = (m[1][0]-m[0][1])*fRoot;
	}
	else
	{
		// |w| <= 1/2
		static size_t s_iNext[3] = { 1, 2, 0 };
		size_t i = 0;
		if ( m[1][1] > m[0][0] )
			i = 1;
		if ( m[2][2] > m[i][i] )
			i = 2;
		size_t j = s_iNext[i];
		size_t k = s_iNext[j];

		fRoot = sqrt(m[i][i]-m[j][j]-m[k][k] + 1.0f);
		float* apkQuat[3] = { &x, &y, &z };
		*apkQuat[i] = 0.5f*fRoot;
		fRoot = 0.5f/fRoot;
		w = (m[k][j]-m[j][k])*fRoot;
		*apkQuat[j] = (m[j][i]+m[i][j])*fRoot;
		*apkQuat[k] = (m[k][i]+m[i][k])*fRoot;
	}
}

void quaternion::to_rotation_matrix( matrix3& m ) const
{
	float tx  = x+x;
	float ty  = y+y;
	float tz  = z+z;
	float twx = tx*w;
	float twy = ty*w;
	float twz = tz*w;
	float txx = tx*x;
	float txy = ty*x;
	float txz = tz*x;
	float tyy = ty*y;
	float tyz = tz*y;
	float tzz = tz*z;

	m[0][0] = 1.0f - ( tyy + tzz );
	m[0][1] = txy - twz;
	m[0][2] = txz + twy;
	m[1][0] = txy + twz;
	m[1][1] = 1.0f - ( txx + tzz );
	m[1][2] = tyz - twx;
	m[2][0] = txz - twy;
	m[2][1] = tyz + twx;
	m[2][2] = 1.0f - ( txx + tyy );
}

void quaternion::from_angle_axis ( float angle, const vector3& axis )
{
	float half_angle = 0.5f * angle;
	float sine = sin( half_angle );

	w = cos( half_angle );
	x = sine * axis.x;
	y = sine * axis.y;
	z = sine * axis.z;
}

void quaternion::to_angle_axis ( float& angle, vector3& axis ) const
{
	float sqrt_length;
	float inv_length;

	sqrt_length = x * x + y * y + z * z;

	if ( sqrt_length > 0.0 )
	{
		angle = 2.0f * acos( w );
		inv_length = 1.0f / sqrt( sqrt_length );
		axis.x = x * inv_length;
		axis.y = y * inv_length;
		axis.z = z * inv_length;
	}
	else
	{
		angle = 0.0f;
		axis.x = 1.0;
		axis.y = 0.0;
		axis.z = 0.0;
	}
}

void quaternion::identity()
{
	w = 1;
	x = 0;
	y = 0;
	z = 0;
}

void quaternion::normalize()
{
	float len, factor;

	len = w * w + x * x + y * y + z * z;
	factor = 1.0f / sqrt( len );

	*this = *this * factor;
}

quaternion quaternion::operator + ( const quaternion& q ) const
{
	return quaternion( w + q.w, x + q.x, y + q.y, z + q.z );
}

quaternion quaternion::operator - ( const quaternion& q ) const
{
	return quaternion( w - q.w, x - q.x, y - q.y, z - q.z);
}

quaternion quaternion::operator * ( const quaternion& q ) const
{
	return quaternion
	(
		w * q.w - x * q.x - y * q.y - z * q.z,
		w * q.x + x * q.w + y * q.z - z * q.y,
		w * q.y + y * q.w + z * q.x - x * q.z,
		w * q.z + z * q.w + x * q.y - y * q.x
	);
}

quaternion quaternion::operator * ( float scalar ) const
{
	return quaternion( scalar * w, scalar * x, scalar * y, scalar * z );
}

quaternion& quaternion::operator = ( const quaternion& q )
{
	w = q.w;
	x = q.x;
	y = q.y;
	z = q.z;

	return *this;
}

bool quaternion::operator == ( const quaternion& q ) const
{
	return ( q.x == x ) && ( q.y == y ) &&
				( q.z == z ) && ( q.w == w );
}

bool quaternion::operator != (const quaternion& q) const
{
	return !operator==( q );
}

void quaternion::from_euler_angles(float theta_x, float theta_y, float theta_z)
{
	float cos_x_2 = cosf(0.5f*theta_x);
	float cos_y_2 = cosf(0.5f*theta_y);
	float cos_z_2 = cosf(0.5f*theta_z);

	float sin_x_2 = sinf(0.5f*theta_x);
	float sin_y_2 = sinf(0.5f*theta_y);
	float sin_z_2 = sinf(0.5f*theta_z);

	// and now compute quaternion
	w   = cos_x_2*cos_y_2*cos_z_2 - sin_x_2*sin_y_2*sin_z_2;
	x = sin_x_2*cos_y_2*cos_z_2 - cos_x_2*sin_y_2*sin_z_2;
	y = sin_x_2*cos_y_2*sin_z_2 + cos_x_2*sin_y_2*cos_z_2;
	z = cos_x_2*cos_y_2*sin_z_2 + sin_x_2*sin_y_2*cos_z_2;
}


quaternion quaternion::inverse() const
{
	float norm = w*w+x*x+y*y+z*z;
	if (norm > 0.0) {
	    float inv = 1.0f/norm;
	    return quaternion(w*inv,-x*inv,-y*inv,-z*inv);
	}
	else
	    return quaternion(0.0f, 0.0f, 0.0f, 0.0f);
}

vector3 quaternion::operator* (const vector3 &v) const
{
	// nVidia SDK implementation
	vector3 uv, uuv;
	vector3 qvec(x, y, z);
	uv = qvec.cross(v);
	uuv = qvec.cross(uv);
	uv *= (2.0f * w);
	uuv *= 2.0f;
	
	return v + uv + uuv;

}